#pragma once
#include<pcl/point_cloud.h>
#include<pcl/point_types.h>
#include<Eigen/Eigen>

using namespace std;
using namespace pcl;
typedef pcl::PointXYZ PointIoType;

class Mapping
{
public:
	Mapping();
	~Mapping();
	bool mapDataInit(int _iterate);

	double getScore(pcl::PointCloud<PointIoType>::Ptr tar_pc,
		pcl::PointCloud<PointIoType>::Ptr sour_pc);
	double getScore();
	double getScore(pcl::PointCloud<PointIoType>::Ptr tar_pc,const Eigen::Matrix4d transformation_matrix);
	Eigen::Matrix4d getTransformation();
	void registeICP(pcl::PointCloud<PointIoType>::Ptr tar_pc,
		pcl::PointCloud<PointIoType>::Ptr sour_pc,
		pcl::PointCloud<PointIoType>::Ptr output);
	void registeNDT(pcl::PointCloud<PointIoType>::Ptr tar_pc,
		pcl::PointCloud<PointIoType>::Ptr sour_pc, 
		pcl::PointCloud<PointIoType>::Ptr output);
	void print4x4Matrix(const Eigen::Matrix4d & matrix);
	void buildGroundTruthMapToBin(const string bin_input_path);
	void buildGroundTruthMapToPCD(const string pcd_input_path);

private:
	int _iterate;
	// Defining a rotation matrix and translation vector
	Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity();
	
	double my_score = 0;
};

